﻿using System;
using System.Collections.Generic;

namespace GeoFits
{
    /// <summary>
    /// 平面及空间直线拟合
    /// </summary>
    public static class LineFit
    {
        /// <summary>
        /// 平面直线拟合 
        /// </summary>
        /// <param name="pts">拟合所用的点串</param>
        /// <param name="is3d">是否是空间拟合：默认为平面拟合</param>
        /// <param name="P">权阵</param>
        /// <param name="config">是否使用抗差最小二乘法</param>
        /// <returns>平面拟合结果</returns>
        /// <remarks>注意：在平面拟合中点的坐标Z是不起作用的</remarks>
        public static FitResult Fit(List<Point> pts, bool is3d = false, Matrix P = null, RLSConfig config = null)
        {
            if (pts.Count < 2)            
                throw new CountOfPointsException();
            Vector dir;
            if (!is3d)
                dir = FitHelper.getDiffVector(pts, false);
            else
                dir= FitHelper.getDiffVector(pts);

            Quaternion qt = Quaternion.getQuaternion(dir.Normalize(), new Vector(1, 0, 0));
            List<Point> newPts = Quaternion.Rotation(pts, qt);

            FitResult result = new FitResult();
            Matrix X = is3d ?
                FitPattern.LinearModule(newPts, LineFitFunctionModule_3dFast, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, P, config) :
                FitPattern.LinearModule(newPts, LineFitFunctionModule_2dFast, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, P, config);
            Point ptAftTrans;
            Vector vtAftTrans;
            if (!is3d)
            {
                ptAftTrans = qt.Inverse().Rotation(new Point(0, X[0, 0], 0));
                //ptQt = qt.Inverse() * new Quaternion(0, 0, X[0, 0], 0);
                //vtQt = qt.Inverse() * new Quaternion(0, 1, X[1, 0], 0);
                vtAftTrans = qt.Inverse().Rotation(new Vector(1, X[1, 0], 0));
            }
            else
            {
                ptAftTrans = qt.Inverse().Rotation(new Point(0, X[0, 0], X[1, 0]));
                vtAftTrans = qt.Inverse().Rotation(new Vector(1, X[2, 0], X[3, 0]));
                //vtQt = qt.Inverse() * new Quaternion(0, 1, X[2, 0], X[3, 0]);
            }
            Line line = new Line(ptAftTrans, vtAftTrans);
            result.ResultGeometry = line;
            return result;
        }
        /// <summary>
        /// 空间直线普通最小二乘法
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中未使用此值）</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        private static void LineFitFunctionModule_3dFast(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(2 * n, 4);
            L = new Matrix(2 * n, 1);

            for (int i = 0; i < n; i++)
            {
                A[i * 2, 0] = 1;
                A[i * 2, 1] = 0;
                A[i * 2, 2] = pts[i].X;
                A[i * 2, 3] = 0;
                A[i * 2 + 1, 0] = 0;
                A[i * 2 + 1, 1] = 1;
                A[i * 2 + 1, 2] = 0;
                A[i * 2 + 1, 3] = pts[i].X;
                L[i * 2, 0] = pts[i].Y;
                L[i * 2 + 1, 0] = pts[i].Z;
            }
        }
        /// <summary>
        /// 平面直线普通最小二乘法
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中未使用此值）</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        private static void LineFitFunctionModule_2dFast(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 2);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                A[i, 0] = 1;
                A[i, 1] = pts[i].X;
                L[i, 0] = pts[i].Y;
            }
        }

        /// <summary>
        /// 整体最小二乘法模型
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        /// <remarks>虽然实现了此种模型，但在实际拟合中并没有使用。由于经实验验证，与普通最小二乘法计算结果相差非常微小，在精密工程测量与工业测量拟合中可以忽略。</remarks>
        private static void LineFitFunctionModule_3dTotal(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, double[] conditions)
        {            
            int n = pts.Count;
            A = new Matrix(n, 4);
            L = new Matrix(n, 1);
            double a = Math.Sqrt(1 - m0[2, 0] * m0[2, 0] - m0[3, 0] * m0[3, 0]);
            for (int i = 0; i < n; i++)
            {
                double dotProduct = pts[i].X * a + m0[2, 0] * (pts[i].Y - m0[0, 0]) + m0[3, 0] * (pts[i].Z - m0[1, 0]);
                double temp = Math.Sqrt(pts[i].X * pts[i].X + (pts[i].Y - m0[0, 0]) * (pts[i].Y - m0[0, 0]) + (pts[i].Z - m0[1, 0]) * (pts[i].Z - m0[1, 0]) - dotProduct * dotProduct);

                A[i, 0] = (m0[2, 0] * dotProduct + m0[0, 0] - pts[i].Y) / temp;

                A[i, 1] = (m0[3, 0] * dotProduct + m0[1, 0] - pts[i].Z) / temp;
                A[i, 2] = -((-m0[2, 0] * pts[i].X / a) + pts[i].Y - m0[0, 0]) * dotProduct / temp;
                A[i, 3] = -((-m0[3, 0] * pts[i].X / a) + pts[i].Z - m0[1, 0]) * dotProduct / temp;

                L[i, 0] = -temp;
            }
        }        
    }
}
